
#include <vector>

#include <opencv2/opencv.hpp>
#include "../include/FaceAlignmentWraper.h"
using namespace std;



FaceAlignmentWraper::FaceAlignmentWraper()
{}

FaceAlignmentWraper::~FaceAlignmentWraper() {}


//FaceAlignment* faceAlignment = NULL;

bool FaceAlignmentWraper::CreateFaceAlignment(const char* modelPath)
{
	char detectModel[] = "libboostsj.dll";
	char trackerModel[] = "libtrackertj.dll";
	this->pxx = new INTRAFACE::XXDescriptor(4);
	this->m_faceAlignment = new INTRAFACE::FaceAlignment(detectModel, trackerModel, pxx);
	return this->m_faceAlignment->Initialized();
}

float FaceAlignmentWraper::GetFacePoints(unsigned char* imgBgr24, int width,
	int height, int widthstep, FaceFeatureRectangle &faceRect, FacePointsModel* pointModel)
{
	int landmarks_count = 0;
	float score, notFace = -1;
	if (this->m_faceAlignment)
	{
		cv::Mat mat(height, width, CV_8UC3, imgBgr24, widthstep);
		if (!mat.empty())
		{

			cv::Rect face;
			face.x = faceRect.left;
			face.y = faceRect.top;
			face.width = faceRect.right - faceRect.left;
			face.height = faceRect.bottom - faceRect.top;
			cv::Mat X;

			if (m_faceAlignment->Detect(mat, face, X, score) == INTRAFACE::IF_OK)
			{

				landmarks_count = X.cols;
				landmarks_count = landmarks_count <= POINT_SIZE ? landmarks_count : POINT_SIZE;
				for (int i = 0; i < landmarks_count; i++)
				{
					pointModel[0].points[i].x = (int)X.at<float>(0, i);
					pointModel[0].points[i].y = (int)X.at<float>(1, i);
				}

			}
			HeadPose headPose;
			if (m_faceAlignment->EstimateHeadPose(mat, headPose) == INTRAFACE::IF_OK)
			{
				pointModel[0].roll = headPose.angles[0];
				pointModel[0].yaw = headPose.angles[1];
				pointModel[0].pitch = headPose.angles[2];
			}

		}

	}
	return score;
}

bool FaceAlignmentWraper::CalculateHeadPose(unsigned char* imgBgr24, int width, int height, int widthstep, FacePointsModel* pointModel)
{
	bool result = false;
	if (this->m_faceAlignment&&pointModel)
	{
		cv::Mat mat(height, width, CV_8UC3, imgBgr24, widthstep);
		if (!mat.empty())
		{
			HeadPose headPose;
			if (m_faceAlignment->EstimateHeadPose(mat, headPose) == INTRAFACE::IF_OK)
			{
				pointModel[0].roll = headPose.angles[0];
				pointModel[0].yaw = headPose.angles[1];
				pointModel[0].pitch = headPose.angles[2];

				result = true;
			}
		}
	}
	return result;
	
}

void FaceAlignmentWraper::Free()
{
	if (this->pxx)
	{
		delete pxx;
		pxx = NULL;
	}
	if (this->m_faceAlignment)
	{
		delete m_faceAlignment;
		m_faceAlignment = NULL;
	}
}


//FaceX* CreateFacePointsEngine(std::string modelPath)
//{
//	FaceX* engine = new FaceX(modelPath);
//	return engine;
//}
//
//int CalculateFacePoints(FaceX* engine, unsigned char* imgBgr24, int width,
//	int height, int widthstep, FaceFeatureRectangle &faceRect, FacePointsModel* pointModel)
//{
//	int landmarks_count = 0;
//	if (engine)
//	{
//		cv::Mat mat(height, width, CV_8UC3, imgBgr24, widthstep);
//		if (!mat.empty())
//		{
//			cv::Mat greyMat;
//			cv::cvtColor(mat, greyMat, CV_BGR2GRAY);
//			landmarks_count = engine->landmarks_count();
//			cv::Rect face;
//			face.x = faceRect.left;
//			face.y = faceRect.top;
//			face.width = faceRect.right - faceRect.left;
//			face.height = faceRect.bottom - faceRect.top;
//			vector<cv::Point2d> landmarks = engine->Alignment(greyMat, face);
//			landmarks_count = landmarks_count <= POINT_SIZE ? landmarks_count : POINT_SIZE;
//			int index = 0;
//
//			//for (vector<cv::Point2d>::iterator landmark = landmarks.begin(); landmark != landmarks.end(); landmark++)
//			//{
//			//	if (index >= landmarks_count)
//			//	{
//			//		break;
//			//	}
//			//	pointModel[0].points[index].x = landmark.;
//			//	pointModel[0].points[index].y = landmark.y;
//			//	index++;
//			//}
//
//			for (int i = 0; i < landmarks_count; i++)
//			{
//				pointModel[0].points[i].x = landmarks[i].x;
//				pointModel[0].points[i].y = landmarks[i].y;
//			}
//
//			//for (cv::Point2d landmark : landmarks)
//			//{
//			//	if (index>= landmarks_count)
//			//	{
//			//		break;
//			//	}
//			//	pointModel[0].points[index].x = landmark.x;
//			//	pointModel[0].points[index].y = landmark.y;
//			//	index++;
//			//}
//		}
//
//	}
//	return landmarks_count;
//}
//
//
//void Destory(FaceX* engine)
//{
//	if (engine)
//	{
//		delete engine;
//		engine = NULL;
//	}
//}
